#ifndef __SSE_ACCELERATION_INL__
#define __SSE_ACCELERATION_INL__

#include <emmintrin.h>
#include <smmintrin.h>
#include "TypeDefine.h"

namespace SSE_OPS {

__forceinline
	__m128i add_4_uint32_sse_nocarry(__m128i a, __m128i b)//needs 5 registers
{
	__m128i r = _mm_add_epi32(a,b);//r
	__m128i f = _mm_cmplt_epi32(r,b);//f=r<b
	__m128i t = _mm_xor_si128(r,b);//r^b
	f = _mm_xor_si128(f,t);//f^(r^b)
	f = _mm_srli_epi32(f,31);//f^(r^b)>>31
	f = _mm_slli_si128(f,4);//f = [f^(r^b)>>31]<<4
	t = _mm_set1_epi32(0xffffffff);
	t = _mm_cmpeq_epi32(r,t);//k = r==0xffffffff
	t = _mm_and_si128(f,t);//f&k
	t = _mm_slli_si128(t,4);//g=[f&k]<<4
	f = _mm_or_si128(f,t);//f=f|g
	r = _mm_add_epi32(r,f);// r+=f

	return r;
}

__forceinline
	void add_4_uint32_sse_carry(const UINT32 *ppIn0, const UINT32 *ppIn1, UINT32 *ppOut, UINT32 &pfflg, UINT32 &pfkflg)
{
	const __m128i* arg1 = (__m128i*)ppIn0;
	const __m128i* arg2 = (__m128i*)ppIn1;

	UINT32 flow0 = pfflg;
	UINT32 fandk = pfkflg;

	__m128i a = _mm_loadu_si128(arg1);//a
	__m128i b = _mm_loadu_si128(arg2);//b

	__m128i r = _mm_add_epi32(a,b);//r
	__m128i f = _mm_cmplt_epi32(r,b);//f=r<b
	__m128i t = _mm_xor_si128(r,b);//r^b
	f = _mm_xor_si128(f,t);//f^(r^b)
	f = _mm_srli_epi32(f,31);//f^(r^b)>>31

	pfflg = _mm_extract_epi32(f,3);
	f = _mm_slli_si128(f,4);//f = [f^(r^b)>>31]<<4
	f = _mm_insert_epi32(f,flow0,0);

	t = _mm_set1_epi32(0xffffffff);
	t = _mm_cmpeq_epi32(r,t);//k = r==0xffffffff
	t = _mm_and_si128(f,t);//f&k

	pfkflg = _mm_extract_epi32(t,3);
	t = _mm_slli_si128(t,4);//g=[f&k]<<4
	t = _mm_insert_epi32(t,fandk,0);

	t = _mm_or_si128(f,t);//f=f|g
	r = _mm_add_epi32(r,t);// r+=f

	__m128i* dst = (__m128i*)ppOut;
	_mm_storeu_si128(dst,r);
}

__forceinline
	void sub_4_uint32_sse_carry(const UINT32 *ppIn0, const UINT32 *ppIn1, UINT32 *ppOut, UINT32 &pfflg, UINT32 &pfkflg)
{
	const __m128i* arg1 = (__m128i*)ppIn0;
	const __m128i* arg2 = (__m128i*)ppIn1;

	UINT32 flow0 = pfflg;
	UINT32 fandk = pfkflg;

	__m128i a = _mm_loadu_si128(arg1);//a
	__m128i b = _mm_loadu_si128(arg2);//b

	__m128i r = _mm_sub_epi32(a,b);//r
	__m128i f = _mm_cmplt_epi32(a,b);//f=a<b
	__m128i t = _mm_xor_si128(a,b);//a^b
	f = _mm_xor_si128(f,t);//f^(a^b)
	f = _mm_srli_epi32(f,31);//f^(a^b)>>31

	pfflg = _mm_extract_epi32(f,3);
	f = _mm_slli_si128(f,4);//f = [f^(r^b)>>31]<<4
	f = _mm_insert_epi32(f,flow0,0);

	t = _mm_setzero_si128();
	t = _mm_cmpeq_epi32(r,t);//k = r==0x0
	t = _mm_and_si128(f,t);//f&k

	pfkflg = _mm_extract_epi32(t,3);
	t = _mm_slli_si128(t,4);//g=[f&k]<<4
	t = _mm_insert_epi32(t,fandk,0);

	t = _mm_or_si128(f,t);//f=f|g
	r = _mm_sub_epi32(r,t);// r-=f

	__m128i* dst = (__m128i*)ppOut;
	_mm_storeu_si128(dst,r);
}

__forceinline
	void mul_64_to_128_sse(const UINT32 pA1, const UINT32 pA0, const UINT32 pB1, const UINT32 pB0, UINT32 *ppOut)
{
	__m128i xmm0 = _mm_set_epi32(0,pA1,0,pA0);//0,a1,0,a0
	__m128i xmm1 = _mm_set_epi32(0,pB1,0,pB0);//0,b1,0,b0

	__m128i xmm2 = _mm_mul_epi32(xmm0,xmm1);  //r_h',r_l'

	__m128i xmm3 = _mm_srai_epi32(xmm0,31);
	__m128i xmm4 = _mm_and_si128(xmm3,xmm1);  //xmm3 is free
	xmm3 = _mm_srai_epi32(xmm1,31);
	__m128i xmm5 = _mm_and_si128(xmm3,xmm0);  //xmm3 is free
	xmm3 = _mm_add_epi32(xmm4,xmm5);  //xmm4,xmm5 are free
	xmm4 = _mm_slli_si128(xmm3,4);    //xmm3,xmm5 are free
	xmm5 = _mm_add_epi32(xmm2,xmm4);  //r_h,r_l

	//xmm2,xmm3,xmm4 are free

	xmm2 = _mm_shuffle_epi32(xmm1,_MM_SHUFFLE(3,0,1,2));//0,b0,0,b1, xmm1 is free

	xmm1 = _mm_mul_epi32(xmm0,xmm2);

	xmm3 = _mm_srai_epi32(xmm0,31);   //repeat calculation!
	xmm4 = _mm_and_si128(xmm3,xmm2);  //xmm3 is free
	xmm3 = _mm_srai_epi32(xmm2,31);
	__m128i xmm6 = _mm_and_si128(xmm3,xmm0);  //xmm0, xmm2, xmm3 are free
	xmm0 = _mm_add_epi32(xmm4,xmm6);  //xmm2, xmm3, xmm4, xmm6 are free
	xmm2 = _mm_slli_si128(xmm0,4);
	xmm3 = _mm_add_epi32(xmm1,xmm2);  //r_m1,r_m2

	//xmm0,xmm1,xmm2,xmm4,xmm6 are free

	xmm0 = _mm_srli_si128(xmm3,4);
	xmm1 = _mm_set_epi32(0,0xffffffff,0xffffffff,0);
	xmm2 = _mm_and_si128(xmm0,xmm1);
	xmm5 = add_4_uint32_sse_nocarry(xmm5,xmm2);

	xmm0 = _mm_slli_si128(xmm3,4);
	xmm2 = _mm_and_si128(xmm0,xmm1);
	xmm5 = add_4_uint32_sse_nocarry(xmm5,xmm2);

	__m128i *pDest = (__m128i*)ppOut;
	_mm_storeu_si128(pDest,xmm5);
}

}

#endif